#include <Arduino.h>
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <SPIFFS.h>


#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>

#include <ArduinoJson.h>

JsonDocument doc;


//SDA（GPIO4）
//SCL（GPIO5）
MPU6050 mpu(0x68);

#define WIFIAP 0 

//0是熄灭，1是闪烁
int ledState=0;

const int pwmPins[] = {2, 3, 10, 6};
const int pwmChannels[] = {0, 1, 2, 3}; // 每个引脚对应的LEDC通道
const int freq = 5000;    // PWM频率(Hz)
const int resolution = 8;  // 8位分辨率(0-255)


AsyncWebServer server(80);
AsyncWebSocket ws("/ws");

//连接成功的指示灯
void ledStateTask(void *pvParameters);
//初始化成功的指示灯
void runTask(void *pvParameters);

void onWsEvent(AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type, void *arg, uint8_t *data, size_t len) {
  if (type == WS_EVT_CONNECT) {
    Serial.printf("WebSocket 客户端 #%u 从 %s 连接\n", client->id(), client->remoteIP().toString().c_str());
    client->text("Hello from ESP32-C3!");
    ledState=1;
    digitalWrite(12,0);
  } else if (type == WS_EVT_DISCONNECT) {
    Serial.printf("WebSocket 客户端 #%u 断开连接\n", client->id());
    ledState=0;
  } else if (type == WS_EVT_DATA) {
    // 处理收到的数据
    AwsFrameInfo *info = (AwsFrameInfo*)arg;
    if (info->final && info->index == 0 && info->len == len && info->opcode == WS_TEXT) {
      data[len] = 0;
      Serial.printf("收到文本数据: %s\n", data);

      DeserializationError error=deserializeJson(doc,data);
      if(!error){
        int rate=doc["rate"].as<int>();
        int gpio=doc["gpio"].as<int>();
        Serial.printf("rate:%d,gpio:%d\r\n",rate,gpio);

        if(gpio&0x01){
          ledcWrite(pwmChannels[0],rate);
        }else{
          ledcWrite(pwmChannels[0],0);
        }

        if((gpio>>1)&0x01){
          ledcWrite(pwmChannels[1],rate);
        }else{
          ledcWrite(pwmChannels[1],0);
        }

        if((gpio>>2)&0x01){
          ledcWrite(pwmChannels[2],rate);
        }else{
          ledcWrite(pwmChannels[2],0);
        }

        if((gpio>>3)&0x01){
          ledcWrite(pwmChannels[3],rate);
        }else{
          ledcWrite(pwmChannels[3],0);
        }



      }

      // int rate=atoi((char*)data);
      // analogWrite(2,rate);
      // analogWrite(3,rate);
      // analogWrite(6,rate);
      // analogWrite(10,rate);
      // 回显给所有客户端
      //ws.textAll((AsyncWebSocketMessageBuffer*)(data));
    }
  }
}

void notFound(AsyncWebServerRequest *request) {
  request->send(404, "text/plain", "Not found");
}

void scanI2C() {
  Serial.println("\nI2C扫描中...");
  byte count = 0;
  
  for (byte i = 8; i < 120; i++) {
    Wire.beginTransmission(i);
    if (Wire.endTransmission() == 0) {
      Serial.print("发现I2C设备，地址: 0x");
      Serial.println(i, HEX);
      count++;
      delay(1);
    }
  }
  
  Serial.println("扫描完成");
  Serial.print("发现设备数: ");
  Serial.println(count);
}





void setup() {
  Serial.begin(115200);

  analogSetAttenuation(ADC_11db);
  analogReadResolution(12);
  // pinMode(2,OUTPUT);
  // pinMode(3,OUTPUT);
  // pinMode(6,OUTPUT);
  // pinMode(10,OUTPUT);
  //analogWriteFrequency(1000); // 设置默认频率为 5kHz
  //analogWriteResolution(10);  // 设置默认分辨率为 10 位（0-1023）
  // analogWrite(2,0);
  // analogWrite(3,0);
  // analogWrite(6,0);
  // analogWrite(10,0);


   // 配置PWM
   for(int i=0; i<4; i++) {
    ledcSetup(pwmChannels[i], freq, resolution); // 设置通道
    ledcAttachPin(pwmPins[i], pwmChannels[i]);   // 将通道绑定到GPIO
    ledcWrite(pwmChannels[i], 0);                // 初始占空比为0
  }
  

  Serial.println("init.....");
  // 初始化SPIFFS
  if(!SPIFFS.begin(true)){
    Serial.println("SPIFFS初始化失败!");
    return;
  }
  Wire.begin(4,5); 
  //Wire.setClock(400000); 
  scanI2C();
  
  mpu.initialize();
  delay(300);
  Serial.print("address:");
  Serial.println(mpu.getDeviceID(),HEX);
    
    // 验证连接
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  
  // 配置传感器
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8); // 加速度计±8g
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // 陀螺仪±500°/s
  mpu.setDLPFMode(MPU6050_DLPF_BW_20); // 低通滤波器21Hz
   // 校准加速度计和陀螺仪
   mpu.CalibrateAccel(6);  // 6次校准
   mpu.CalibrateGyro(6);   // 6次校准
   Serial.println("校准完成");

  
  #if WIFIAP==1
    WiFi.setTxPower(WIFI_POWER_19_5dBm);
    WiFi.softAP("fly", "fly123456");
    Serial.print("AP IP 地址: ");
    Serial.println(WiFi.softAPIP());
  #else
    WiFi.begin("CMCC-e73b", "ku7e73tg");
    while (WiFi.status() != WL_CONNECTED) delay(50);
    Serial.print("服务器IP: ");
    Serial.println(WiFi.localIP());
  #endif
  


  // 初始化WebSocket
  ws.onEvent(onWsEvent);
  server.addHandler(&ws);
  //直接读取静态资源文件
  server.serveStatic("/", SPIFFS, "/").setDefaultFile("index.html");
  server.onNotFound(notFound);

  // 启动服务器
  server.begin();


  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);
  xTaskCreate(
    ledStateTask,    // 任务函数
    "MyTask",         // 任务名称
    4096,             // 堆栈大小(字节)
    NULL,             // 任务参数
    1,                // 优先级(0-24，数字越大优先级越高)
    NULL              // 任务句柄(可选)
  );

  xTaskCreate(
    runTask,    // 任务函数
    "run",         // 任务名称
    4096,             // 堆栈大小(字节)
    NULL,             // 任务参数
    1,                // 优先级(0-24，数字越大优先级越高)
    NULL              // 任务句柄(可选)
  );
 

}
//led状态的任务
void ledStateTask(void *pvParameters){
  int state=0;
  while (1)
  {
    if(ledState==1){
      digitalWrite(12,state);
      if(state==0){
        state=1;
      }else{
        state=0;
      }
    }
    vTaskDelay(500);
  }

  vTaskDelete(NULL);
  
}


//是否初始化成功
void runTask(void *pvParameters){
  int state=0;
  while (1)
  {

    digitalWrite(12,state);
    if(state==0){
      state=1;
    }else{
      state=0;
    }
    vTaskDelay(1000);
  }

  vTaskDelete(NULL);
  
}

// 使用多次采样取平均值
float readAverageVoltage(int pin, int samples = 32) {
  long sum = 0;
  for (int i = 0; i < samples; i++) {
    sum += analogRead(pin);
    delay(1);
  }
  float average = sum / (float)samples;
  return average*2*0.88 * (3.3 / 4095.0);
}
int16_t ax, ay, az, gx, gy, gz;
// 姿态数据
float yaw, pitch, roll;

float dt = 0.01;  // 采样时间间隔(秒)
float alpha = 0.96;  // 互补滤波系数
char buff[200];
void loop() {
  static uint32_t prev_ms = millis();
  // 清理断开连接的客户端
  ws.cleanupClients();
  float voltage = readAverageVoltage(0,10);
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accel_scale = 2.0 / 32768.0;
  float gyro_scale = 250.0 / 32768.0;
  sprintf(buff,"{\"accel\":{\"x\":%f,\"y\":%f,\"z\":%f},\"gyro\":{\"x\":%f,\"y\":%f,\"z\":%f},\"vol\":%.2f}",
    ax*accel_scale,ay*accel_scale,az*accel_scale,gx*gyro_scale,gy*gyro_scale,gz*gyro_scale,voltage);
  //Serial.println(buff);
  //ws.textAll(buff);

  
  
   // 计算实际时间间隔
   uint32_t curr_ms = millis();
   dt = (curr_ms - prev_ms) / 1000.0;
   prev_ms = curr_ms;
   
   // 加速度计数据转换为g单位 (±2g范围)
   float accX = ax / 16384.0;
   float accY = ay / 16384.0;
   float accZ = az / 16384.0;
   
   // 从加速度计计算姿态(弧度)
   float accPitch = atan2(-accX, sqrt(accY*accY + accZ*accZ));
   float accRoll = atan2(accY, accZ);
   
   // 陀螺仪数据转换为度/秒 (±250°/s范围)
   float gyroX = gx / 131.0;
   float gyroY = gy / 131.0;
   float gyroZ = gz / 131.0;
   
   // 互补滤波
   pitch = alpha * (pitch + gyroY * dt) + (1.0 - alpha) * accPitch;
   roll = alpha * (roll + gyroX * dt) + (1.0 - alpha) * accRoll;
   
   // 转换为角度
   float pitch_deg = pitch * 180.0 / M_PI;
   float roll_deg = roll * 180.0 / M_PI;
   
   // 计算偏航角(需要磁力计)
   static float yaw = 0;
   yaw += gyroZ * dt;


   sprintf(buff,"{\"roll\":%f,\"pitch\":%f,\"yaw\":%f,\"vol\":%.2f}",roll,pitch,yaw,voltage);
   ws.textAll(buff);


  delay(300);

}

